#include "Sliding.h"
#include "math.h"

static float SMC_Signal(float s);
static float SMC_Sat(cSMC* smc, float s);
static void SMC_OutContinuation(cSMC* smc);

// 初始化
void SMC_Init(cSMC* smc)
{
    if(smc == NULL)
        return;

    smc->param.J = 0.0f;
    smc->param.K = 0.0f;
    smc->param.c = 0.0f;
    smc->param.epsilon = 0.0f;
    smc->flag = EXPONENT;
    smc->u_max = 0.0f;
    smc->limit = 0.0f;

    smc->error.tar_now = 0.0f;
    smc->error.tar_last = 0.0f;
    smc->error.tar_differential = 0.0f;

    smc->error.p_error = 0.0f;
    smc->error.v_error = 0.0f;
    smc->error.v_error_integral = 0.0f;
    smc->error.pos_error_eps = 0.0f;
    smc->error.vol_error_eps = 0.0f;
    smc->error.pos_get = 0.0f;
    smc->error.vol_get = 0.0f;
}

// EXPONENT参数设置,线性滑模面，指数趋近率，Yaw
void SMC_SetParamExp(cSMC* smc, float J, float K, float c, float epsilon, float limit, float u_max, Rmode flag,
                     float pos_esp)
{
    if(smc == NULL)
        return;

    smc->param.J = J;
    smc->param.K = K;
    smc->param.c = c;
    smc->error.pos_error_eps = pos_esp;
    smc->flag = flag;
    smc->param.epsilon = epsilon;
    smc->u_max = u_max;
    smc->limit = limit;
    SMC_OutContinuation(smc);
}

// TFSMC参数设置
void SMC_SetParamTFSMC(cSMC* smc, float J, float K, float p, float q, float beta, float epsilon, float limit,
                       float u_max, Rmode flag, float pos_esp)
{
    if(smc == NULL)
        return;

    smc->param.J = J;
    smc->param.K = K;
    smc->param.p = p;
    smc->param.q = q;
    smc->error.pos_error_eps = pos_esp;
    smc->param.beta = beta;
    smc->flag = flag;
    smc->param.epsilon = epsilon;
    smc->u_max = u_max;
    smc->limit = limit;
    SMC_OutContinuation(smc);
}

// EISMC参数设置,比例积分滑模面，指数趋近律，位置控制，Pitch，拨弹轮，速度控制，摩擦轮
void SMC_SetParamEISMC(cSMC* smc, float J, float K, float c1, float c2, float epsilon, float limit, float u_max,
                       Rmode flag, float pos_esp)
{
    if(smc == NULL)
        return;

    smc->param.J = J;
    smc->param.K = K;
    smc->param.c1 = c1;
    smc->param.c2 = c2;
    smc->error.pos_error_eps = pos_esp;
    smc->flag = flag;
    smc->param.epsilon = epsilon;
    smc->u_max = u_max;
    smc->limit = limit;
    SMC_OutContinuation(smc);
}

// 位置环误差更新
void SMC_ErrorUpdatePos(cSMC* smc, float target, float pos_now, float vol_now)
{
    if(smc == NULL)
        return;

    smc->error.tar_now = target;
    smc->error.tar_differential = (smc->error.tar_now - smc->error.tar_last) / SAMPLE_PERIOD;
    smc->error.tar_differential_second =
        (smc->error.tar_differential - smc->error.tar_differential_last) / SAMPLE_PERIOD;

    smc->error.p_error = pos_now - target;
    smc->error.v_error = vol_now - smc->error.tar_differential;
    smc->error.tar_last = smc->error.tar_now;
    smc->error.p_error_integral += smc->error.p_error * SAMPLE_PERIOD;
    smc->error.tar_differential_last = smc->error.tar_differential;
}

// 速度环误差更新
void SMC_ErrorUpdateVel(cSMC* smc, float target, float vol_now)
{
    if(smc == NULL)
        return;

    smc->error.tar_now = target;
    smc->error.tar_differential = (smc->error.tar_now - smc->error.tar_last) / SAMPLE_PERIOD;
    smc->error.v_error = vol_now - smc->error.tar_now;
    smc->error.v_error_integral += smc->error.v_error * SAMPLE_PERIOD;
    smc->error.tar_last = smc->error.tar_now;
}

// 控制器计算中间值全清空
void SMC_Clear(cSMC* smc)
{
    if(smc == NULL)
        return;

    smc->error.tar_now = 0.0f;
    smc->error.tar_last = 0.0f;
    smc->error.tar_differential = 0.0f;

    smc->error.p_error = 0.0f;
    smc->error.v_error = 0.0f;
    smc->error.v_error_integral = 0.0f;
    smc->error.pos_error_eps = 0.0f;
    smc->error.vol_error_eps = 0.0f;
    smc->error.pos_get = 0.0f;
    smc->error.vol_get = 0.0f;

    smc->error.tar_differential_second = 0.0f;
    smc->error.tar_differential_last = 0.0f;
    smc->error.p_error_integral = 0.0f;
}

// 积分部分误差清空
void SMC_Integval_Clear(cSMC* smc)
{
    if(smc == NULL)
        return;

    smc->error.v_error_integral = 0.0f;
    smc->error.p_error_integral = 0.0f;
}

// 控制器计算函数
float SMC_Calculate(cSMC* smc)
{
    if(smc == NULL)
        return 0.0f;

    float u = 0.0f, fun = 0.0f;

    switch(smc->flag)
    {
        case EXPONENT:  // 线性滑模面，指数趋近率，Yaw
            if(fabsf(smc->error.p_error) - smc->error.pos_error_eps < 0.0f)
            {
                smc->error.p_error = 0.0f;
                return 0.0f;
            }

            smc->s = smc->param.c * smc->error.p_error + smc->error.v_error;
            fun = SMC_Sat(smc, smc->s);
            u = smc->param.J
                * ((-smc->param.c * smc->error.v_error) - smc->param.K * smc->s - smc->param.epsilon * fun);
            break;

        case POWER:  // 线性滑模面，幂次趋近率，Yaw
            if(fabsf(smc->error.p_error) - smc->error.pos_error_eps < 0.0f)
            {
                smc->error.p_error = 0.0f;
                return 0.0f;
            }

            smc->s = smc->param.c * smc->error.p_error + smc->error.v_error;
            fun = SMC_Sat(smc, smc->s);
            u = smc->param.J
                * ((-smc->param.c * smc->error.v_error) - smc->param.K * smc->s
                   - smc->param.K * (powf(fabsf(smc->s), smc->param.epsilon)) * fun);
            break;

        case TFSMC:  // 快速终端滑模控制
            if(fabsf(smc->error.p_error) - smc->error.pos_error_eps < 0.0f)
            {
                smc->error.p_error = 0.0f;
                return 0.0f;
            }

            float pos_pow = powf(fabsf(smc->error.p_error), smc->param.q / smc->param.p);
            if(smc->error.p_error <= 0.0f)
                pos_pow = -pos_pow;

            smc->s = smc->param.beta * pos_pow + smc->error.v_error;
            fun = SMC_Sat(smc, smc->s);

            if(smc->error.p_error != 0.0f)
            {
                u = smc->param.J
                    * (smc->error.tar_differential_second - smc->param.K * smc->s - smc->param.epsilon * fun
                       - smc->error.v_error * ((smc->param.q * smc->param.beta) * pos_pow)
                             / (smc->param.p * smc->error.p_error));
            }
            else
            {
                u = 0.0f;
            }
            break;

        case VELSMC:  // 比例积分滑模面，指数趋近律，速度控制，摩擦轮，底盘电机
            smc->s = smc->error.v_error + smc->param.c * smc->error.v_error_integral;
            fun = SMC_Sat(smc, smc->s);
            u = smc->param.J
                * (smc->error.tar_differential - (smc->param.c * smc->error.v_error) - smc->param.K * smc->s
                   - smc->param.epsilon * fun);
            break;

        case EISMC:  // 比例积分滑模面，指数趋近律，位置控制，Pitch，拨弹轮
            if(fabsf(smc->error.p_error) - smc->error.pos_error_eps < 0.0f)
            {
                smc->error.p_error = 0.0f;
                return 0.0f;
            }

            smc->s =
                smc->param.c1 * smc->error.p_error + smc->error.v_error + smc->param.c2 * smc->error.p_error_integral;
            fun = SMC_Sat(smc, smc->s);
            u = smc->param.J
                * ((-smc->param.c1 * smc->error.v_error) - smc->param.c2 * smc->error.p_error - smc->param.K * smc->s
                   - smc->param.epsilon * fun);
            break;
    }

    smc->error.error_last = smc->error.p_error;  // 更新上一步的误差

    // 控制量限幅
    if(u > smc->u_max)
    {
        u = smc->u_max;
    }
    if(u < -smc->u_max)
    {
        u = -smc->u_max;
    }
    smc->u = u;
    return u;
}

float SMC_Out(cSMC* smc)
{
    if(smc == NULL)
        return 0.0f;
    return smc->u;
}

void SMC_SetOut(cSMC* smc, float out)
{
    if(smc == NULL)
        return;
    smc->u = out;
}

// const Sliding* SMC_GetSmc(const cSMC* smc)
// {
//     if(smc == NULL)
//         return NULL;
//     return &smc->smc;
// }

// 输出连续化
static void SMC_OutContinuation(cSMC* smc)
{
    if(smc == NULL)
        return;

    if(smc->param.K != 0.0f && smc->param.c2 != 0.0f)
    {
        smc->error.p_error_integral =
            (smc->param_last.K / smc->param.K) * (smc->param_last.c2 / smc->param.c2) * smc->error.p_error_integral;
        smc->error.v_error_integral =
            (smc->param_last.K / smc->param.K) * (smc->param_last.c / smc->param.c) * smc->error.v_error_integral;
    }
    smc->param_last = smc->param;
}

// 符号函数
static float SMC_Signal(float s)
{
    if(s > 0.0f)
        return 1.0f;
    else if(s == 0.0f)
        return 0.0f;
    else
        return -1.0f;
}

// 饱和函数
static float SMC_Sat(cSMC* smc, float s)
{
    if(smc == NULL)
        return 0.0f;

    float y = s / smc->param.epsilon;
    if(fabsf(y) <= smc->limit)
        return y;
    else
        return SMC_Signal(y);
}
